//
// Created by Ivor_ on 2022/3/15.
//

#include "nonLinearEquations.h"

double* nleSearch(double start, double end, double stepSize, int rootLimit, double* constantParameters, double (*pEquations)()) {
    double* roots = (double*)calloc(rootLimit, sizeof(double));
    double direction = ((start > end) ? -1. : +1.);
    double scanner = start;
    int count = 0;
    while ((scanner + direction*fabs(stepSize)) < end && count < rootLimit) {
        scanner += direction*fabs(stepSize);
        if (pEquations(scanner - direction*fabs(stepSize), constantParameters) * pEquations(scanner, constantParameters) <= 0.) {
            roots[count++] = scanner;
        }
    }
    return roots;
}

double nleBisection(double start, double end, double eps, double* constantParameters, double (*pEquations)()) {
    double root[2] = {start, end};
    do {
        double half = (root[0] + root[1])/2.;
        if (pEquations(root[0], constantParameters)*pEquations(half, constantParameters) <= 0.) {
            root[1] = half;
        } else {
            root[0] = half;
        }
    } while (fabs(root[0] - root[1]) > eps);
    return (root[0]);
}

double nleIteration(double iniVal, double eps, double* constantParameters, double (*pIterationEquation)()) {
    double root[2] = {1.e+24, iniVal};
    do {
        root[0] = root[1];
        root[1] = pIterationEquation(root[0], constantParameters);
    } while (fabs(root[0] - root[1]) > eps);
    return (root[1]);
}

double nleNewton(double iniVal, double eps, double* constantParameters, double (*pEquation)(), double (*pEqDerivative)()) {
    double root[2] = {1.e+24, iniVal};
    double objVal = pEqDerivative(iniVal, constantParameters);
    do {
        root[0] = root[1];
        root[1] -= pEquation(root[0], constantParameters)/pEqDerivative(root[0], constantParameters);
        objVal = pEqDerivative(root[1], constantParameters);
    } while (fabs(root[0] - root[1]) > eps || fabs(objVal) > eps);
    return (root[1]);
}

double nleChordCut(double initVals[2], double eps, double* constantParameters, double (*pEquation)()) {
    double root[2] = {initVals[0], initVals[1]};
    double objVal = pEquation(root[1], constantParameters) - pEquation(root[0], constantParameters);
    do {
        double temp = root[1];
        root[1] -= pEquation(root[0], constantParameters) * (root[1] - root[0]) / objVal;
        root[0] = temp;
        objVal = pEquation(root[1], constantParameters) - pEquation(root[0], constantParameters);
    } while (fabs(root[0] - root[1]) > eps || fabs(objVal) > eps);
    return (root[1]);
}

double* nleNewtonForEqitions(int dim, double* initVals, double eps, double* constantParameters, double (* *pEquations)() , double (** *jacobiMatrix)()) {
    double* roots = (double*)calloc(dim, sizeof(double));
    memcpy(roots, initVals, dim * sizeof(double));
    double** jacobiMatrixValue = (double**)calloc(dim, sizeof(double*));
    for (int i = 0; i < dim; ++i) {
        jacobiMatrixValue[i] = (double*)calloc(dim, sizeof(double*));
    }
    double* deltaRoots = (double*)calloc(dim, sizeof(double));
    double* functionValues = (double*)calloc(dim, sizeof(double));
    int check;
    do {
        for (int i = 0; i < dim; ++i) {
            functionValues[i] = -(pEquations[i])(roots, constantParameters);
            for (int j = 0; j < dim; ++j) {
                jacobiMatrixValue[i][j] = (jacobiMatrix[i][j])(roots, constantParameters);
            }
        }
        double** jInv = dInverse(dim, jacobiMatrixValue);
        memset(deltaRoots, 0x0, dim * sizeof(double));
        for (int i = 0; i < dim; ++i) {
            for (int j = 0; j < dim; ++j) {
                deltaRoots[i] += jInv[i][j] * functionValues[j];
            }
        }
        freeDMat(dim, jInv);
        check = 0;
        for (int i = 0; i < dim; ++i) {
            roots[i] += deltaRoots[i];
            if (fabs(deltaRoots[i]) > eps) {
                check = 1;
            }
        }
    } while (check);
    for (int i = 0; i < dim; ++i) {
        free(jacobiMatrixValue[i]);
    }
    free(jacobiMatrixValue), free(deltaRoots), free(functionValues);
    return roots;
}
